
/*
* 时间：2021.07.25
* 代码来源：公众号【从0到1的点云】
* 功能：实现OBB包围盒
*/
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/filter.h>
using namespace std;

int main()
{

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::io::loadPCDFile("bunny.pcd", *cloud);
  std::vector<int> map;
  pcl::removeNaNFromPointCloud(*cloud,*cloud,map);
  
  pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
  feature_extractor.setInputCloud(cloud);
  feature_extractor.compute();

  vector <float> moment_of_inertia;//存储惯性矩的特征向量
  vector <float> eccentricity;//存储偏心率的特征向量
  //声明存储描述符和边框所需的所有必要变量。
  pcl::PointXYZ min_point_OBB;
  pcl::PointXYZ max_point_OBB;
  pcl::PointXYZ position_OBB;
  Eigen::Matrix3f rotational_matrix_OBB;
  float major_value, middle_value, minor_value;
  Eigen::Vector3f major_vector, middle_vector, minor_vector;
  Eigen::Vector3f mass_center;

  feature_extractor.getMomentOfInertia(moment_of_inertia);//惯性矩特征
  feature_extractor.getEccentricity(eccentricity);//偏心率特征
  feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
  //OBB对应的相关参数
  feature_extractor.getEigenValues(major_value, middle_value, minor_value);//三个特征值
  feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//三个特征向量
  feature_extractor.getMassCenter(mass_center);//点云中心坐标
                

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("OBB"));
  viewer->setBackgroundColor(0, 0, 0);

  viewer->addPointCloud<pcl::PointXYZ>(cloud, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud, 0, 255, 0),
    "OBBcloud");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "OBBcloud");
  Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
  //OBB
  cout << "position_OBB: " << position_OBB << endl;
  cout << "mass_center: " << mass_center << endl;
  Eigen::Quaternionf quat(rotational_matrix_OBB);
  viewer->addCube(position, 
          quat,
          (double)max_point_OBB.x - (double)min_point_OBB.x,
          (double)max_point_OBB.y - (double)min_point_OBB.y,
          (double)max_point_OBB.z - (double)min_point_OBB.z,
          "OBB");
  viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "OBB");
  viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "OBB");
  
  viewer->setRepresentationToWireframeForAllActors();
  pcl::PointXYZ center(mass_center(0), mass_center(1), mass_center(2));
  pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
  pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
  pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

  cout << "size of cloud :" << cloud->points.size() << endl;
  cout << "moment_of_inertia :" << moment_of_inertia.size() << endl;
  cout << "eccentricity :" << eccentricity.size() << endl;
  viewer->spin();

  return (0);
}